#include <protocol.hpp>

Protocol::Protocol()
{
}

Protocol::~Protocol(void)
{
}

int Protocol::Pack(char* out, int* outSize, char* pData, int dataSize, uint16_t dataSeq, uint16_t dataType)
{
	if (out == nullptr || outSize == nullptr)
	{
		return -1;
	}

	int len = 0;

	uint32_t data_len = (uint32_t)dataSize;
	if (data_len >= BUFF_FIFO_SIZE)
	{
		return -1;
	}

	// head
	out[len++] = P_HEAD_0;
	out[len++] = P_HEAD_1;

	// seq
	out[len++] = (dataSeq >> 0) & 0xFF;
	out[len++] = (dataSeq >> 8) & 0xFF;

	// type
	out[len++] = (dataType >> 0) & 0xFF;
	out[len++] = (dataType >> 8) & 0xFF;

	// length
	out[len++] = (dataSize >> 0) & 0xFF;
	out[len++] = (dataSize >> 8) & 0xFF;
	out[len++] = (dataSize >> 16) & 0xFF;
	out[len++] = (dataSize >> 24) & 0xFF;

	// data
	for (int i = 0; i < dataSize; i++)
	{
		out[len++] = pData[i];
	}

	// crc
	uint32_t crc = Calc32Crc(out, len);
	out[len++]	 = (crc >> 0) & 0xFF;
	out[len++]	 = (crc >> 8) & 0xFF;
	out[len++]	 = (crc >> 16) & 0xFF;
	out[len++]	 = (crc >> 24) & 0xFF;

	// foot
	out[len++] = P_FOOT_0;
	out[len++] = P_FOOT_1;

	*outSize = len;

	return 0;
}

int Protocol::Unpack(char* out, int* outSize, uint16_t* dataSeq, uint16_t* dataType, BuffFifo* buffFifo)
{
	char* p_buff = buffFifo->GetBuff();
	if (p_buff == NULL)
	{
		return -1;
	}

	uint16_t ret_seq  = 0;
	uint16_t ret_type = 0;
	int ret_len		  = 0;
	int ret_ind		  = 0;
	uint32_t ret_crc  = 0;
	bool ret_finish	  = false;

	int len	 = 0;
	int step = 0;
	int cnt	 = buffFifo->Size();
	if (cnt <= 0)
	{
		return -1;
	}

	int foot = buffFifo->GetFoot();
	for (int i = 0; i < cnt; i++)
	{
		uint8_t ch = p_buff[foot];
		foot++;
		foot %= BUFF_FIFO_SIZE;

		switch (step)
		{
			case 0: // HEAD_0
			{
				ret_ind = 0;
				step	= 0;
				if (ch == P_HEAD_0)
				{
					out[len++] = ch;
					ret_ind++;
					step = 1;
				}
				break;
			}
			case 1: // HEAD_1
			{
				step = 0;
				if (ch == P_HEAD_1)
				{
					out[len++] = ch;
					ret_ind++;
					step = 2;
				}
				break;
			}
			case 2: // SEQ_0
			{
				out[len++] = ch;
				ret_ind++;
				ret_seq = (ch << 0);
				step	= 3;
				break;
			}
			case 3: // SEQ_1
			{
				out[len++] = ch;
				ret_ind++;
				ret_seq |= (ch << 8);
				step = 4;
				break;
			}
			case 4: // TYPE_0
			{
				out[len++] = ch;
				ret_ind++;
				ret_type = (ch << 0);
				step	 = 5;
				break;
			}
			case 5: // TYPE_1
			{
				out[len++] = ch;
				ret_ind++;
				ret_type |= (ch << 8);
				step = 6;
				break;
			}
			case 6: // DATA_LEN_HIGH_8BITS
			{
				out[len++] = ch;
				ret_ind++;
				ret_len = (ch << 0);
				step	= 7;
				break;
			}
			case 7: // DATA_LEN_LOW_8BITS
			{
				out[len++] = ch;
				ret_ind++;
				ret_len |= (ch << 8);
				step = 8;
				break;
			}
			case 8: // DATA_LEN_HIGH_8BITS
			{
				out[len++] = ch;
				ret_ind++;
				ret_len |= (ch << 16);
				step = 9;
				break;
			}
			case 9: // DATA_LEN_LOW_8BITS
			{
				out[len++] = ch;
				ret_ind++;
				ret_len |= (ch << 24);
				step = 10;
				if (ret_len == 0)
				{
					step = 11;
				}
				break;
			}
			case 10: // DATA
			{
				step	   = 10;
				out[len++] = ch;
				ret_ind++;
				if (ret_ind >= ret_len + P_SIZE_HEAD + P_SIZE_SEQ + P_SIZE_TYPE + P_SIZE_LEN)
				{
					step = 11;
				}
				break;
			}
			case 11: // CRC_0
			{
				ret_crc = (ch << 0);
				step	= 12;
				break;
			}
			case 12: // CRC_1
			{
				ret_crc |= (ch << 8);
				step = 13;
				break;
			}
			case 13: // CRC_2
			{
				ret_crc |= (ch << 16);
				step = 14;
				break;
			}
			case 14: // CRC_3
			{
				ret_crc |= (ch << 24);
				step = 15;
				break;
			}
			case 15: // FOOT_0
			{
				step = 0;
				if (ch == P_FOOT_0)
				{
					step = 16;
				}
				break;
			}
			case 16: // FOOT_1
			{
				step = 0;
				if (ch == P_FOOT_1)
				{
					ret_finish = true;
				}
				break;
			}
			default:
			{
				break;
			}
		}
		if (ret_finish)
		{
			break;
		}
	}

	if (!ret_finish)
	{
		return -1;
	}

	*dataType = ret_type;
	*dataSeq  = ret_seq;

	uint32_t crc = Calc32Crc(out, len);
	buffFifo->SetFoot(foot);

	if (crc != ret_crc)
	{
		return -1;
	}

	char* p = out;
	char* q = out + P_SIZE_HEAD + P_SIZE_SEQ + P_SIZE_TYPE + P_SIZE_LEN;
	for (int i = 0; i < ret_len; i++)
	{
		*p++ = *q++;
	}
	*outSize = ret_len;

	return 0;
}

uint32_t Protocol::Crc32Value(uint32_t i)
{
	uint32_t j;
	uint32_t crc;
	crc = i;
	for (j = 8; j > 0; j--)
	{
		if (crc & 1)
		{
			crc = (crc >> 1) ^ CRC32_SEED_VAL;
		}
		else
		{
			crc >>= 1;
		}
	}
	return crc;
}

uint32_t Protocol::Calc32Crc(char* pData, int size)
{
	if (size <= 0)
	{
		return 0;
	}

	uint32_t temp1;
	uint32_t temp2;
	uint32_t crc = 0;
	for (int i = 0; i < size; i++)
	{
		temp1		 = (crc >> 8) & 0x00FFFFFFL;
		uint8_t data = (uint8_t)pData[i];
		temp2		 = Crc32Value(((uint32_t)crc ^ data) & 0xff);
		crc			 = temp1 ^ temp2;
	}
	return crc;
}